/*
 * SsrrExample.cpp
 *
 *  Created on: Oct 22, 2013
 *      Author: incubed
 */

#include <ompl/tools/benchmark/Benchmark.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/planners/kpiece/KPIECE1.h>
#include <ssrr_ompl_planner/SsrrPlanning.h>
//#include <omplapp/config.h>
#include <boost/math/constants/constants.hpp>


using namespace ssrr;

class SSRRStateSampler : public ompl::base::ValidStateSampler
{
public:
    SSRRStateSampler(const ompl::base::SpaceInformation *si) : ValidStateSampler(si)
    {
        name_ = "dummy ssrr sampler";
    }
    // Generate a sample in the valid part of the R^3 state space
    // Valid states satisfy the following constraints:
    // -1<= x,y,z <=1
    // if .25 <= z <= .5, then |x|>.8 and |y|>.8
    virtual bool sample(ompl::base::State *state)
    {
        double* val = static_cast<ompl::base::RealVectorStateSpace::StateType*>(state)->values;
        double z = rng_.uniformReal(-1,1);

        if (z>.25 && z<.5)
        {
            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
            switch(rng_.uniformInt(0,3))
            {
            case 0: val[0]=x-1;  val[1]=y-1;
                break;
            case 1: val[0]=x-.8; val[1]=y+.8;
                break;
            case 2: val[0]=y-1;  val[1]=x-1;
                break;
            case 3: val[0]=y+.8; val[1]=x-.8;
                break;
            }
        }
        else
        {
            val[0] = rng_.uniformReal(-1,1);
            val[1] = rng_.uniformReal(-1,1);
        }
        val[2] = z;
        std::cout<< "return sample " << val[0] << " " << val[1] << " " << val[2] << std::endl;
        assert(si_->isValid(state));
        return true;
    }
    // We don't need this in the example below.
    virtual bool sampleNear(ompl::base::State*, const ompl::base::State*, const double)
    {
        throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
        return false;
    }
protected:
    ompl::RNG rng_;
};

// return an instance of my sampler
ompl::base::ValidStateSamplerPtr allocSSRRStateSampler(const ompl::base::SpaceInformation *si)
{
    return ompl::base::ValidStateSamplerPtr(new SSRRStateSampler(si));
}

bool isValid(const ompl::base::State* state) {
	const double* val = static_cast<const ompl::base::RealVectorStateSpace::StateType*>(state)->values;
	double x = val[0];
	double y = val[1];
	double th = val[2];
	return true;
}

void ssrrSetup(SsrrPlanning& setup, int samplerIndex)
{
    // plan for kinematic car in SE(2)
    ompl::base::StateSpacePtr SE2(setup.getStateSpace());

    // set the bounds for the R^2 part of SE(2)
    ompl::base::RealVectorBounds bounds(2);
    bounds.setLow(-10); // derive from map
    bounds.setHigh(10);
    SE2->as<ompl::base::SE2StateSpace>()->setBounds(bounds);

    // define start state
    ompl::base::ScopedState<ompl::base::SE2StateSpace> start(SE2);
    start->setX(-3.);
    start->setY(0);
    start->setYaw(0);

    // define goal state
    ompl::base::ScopedState<ompl::base::SE2StateSpace> goal(SE2);
    goal->setX(2);
    goal->setY(2);
    goal->setYaw(boost::math::constants::pi<double>());


	// set the start & goal states
	setup.setStartAndGoalStates(start, goal, .1);
	if (samplerIndex == 1) {
		setup.setStateValidityChecker(boost::bind(&isValid, _1));
	} else if (samplerIndex==2) {
		// use dummy sampler
        setup.getSpaceInformation()->setValidStateSamplerAllocator(allocSSRRStateSampler);
	}
}

void ssrrDemo(SsrrPlanning& setup)
{
    setup.setPlanner(ompl::base::PlannerPtr(new ompl::control::KPIECE1(setup.getSpaceInformation())));
    std::vector<double> cs(2);
    cs[0] = cs[1] = 0.1;
    setup.setup();
    setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);

    // try to solve the problem
    if (setup.solve(20))
    {
        // print the (approximate) solution path: print states along the path
        // and controls required to get from one state to the next
        ompl::control::PathControl& path(setup.getSolutionPath());
        //path.interpolate(); // uncomment if you want to plot the path
        path.print(std::cout);
        if (!setup.haveExactSolutionPath())
        {
            std::cout << "Solution is approximate. Distance to actual goal is " <<
                         setup.getProblemDefinition()->getSolutionDifference() << std::endl;
        }
    }

}
void ssrrBenchmark(SsrrPlanning& setup)
{
    ompl::tools::Benchmark::Request request(20., 10000., 10); // runtime (s), memory (MB), run count

    ompl::tools::Benchmark b(setup, setup.getName());
    b.addPlanner(ompl::base::PlannerPtr(new ompl::control::RRT(setup.getSpaceInformation())));
    b.addPlanner(ompl::base::PlannerPtr(new ompl::control::KPIECE1(setup.getSpaceInformation())));
    b.benchmark(request);
    b.saveResultsToFile();
}
/*
int main(int argc, char**)
{
<<<<<<< .mine
    SsrrPlanning regularCar;
=======
	SsrrPlanning regular_robot;
>>>>>>> .r86

<<<<<<< .mine
    ssrrSetup(regularCar, 2);
=======
	ssrrSetup(regular_robot, 2);
>>>>>>> .r86

<<<<<<< .mine
    // If any command line arguments are given, solve the problem multiple
    // times with different planners and collect benchmark statistics.
    // Otherwise, solve the problem once for each car type and print the path.
    if (argc>1)
        ssrrBenchmark(regularCar);
    else
        ssrrDemo(regularCar);
    return 0;
=======
	// If any command line arguments are given, solve the problem multiple
	// times with different planners and collect benchmark statistics.
	// Otherwise, solve the problem once for each car type and print the path.
	if (argc>1)
		ssrrBenchmark(regular_robot);
	else
		ssrrDemo(regular_robot);
	return 0;
>>>>>>> .r86
}
*/
